#include "bsp/board_config.h"
#include "serial.h"
#include "bsp/uart.h"
#include "protocol.h"
#include "utils/utils.h"
#include "utils/crc16.h"
#include "iap.h"
// ================================================================================

#if CONFIG_SERIAL_TYPE > SERIAL_NONE
static u16 serial_tx_crc16;

#if CONFIG_SERIAL_TYPE == SERIAL_BOOTLOADER
//static can_pack_t iap_can_pack;
can_pack_t can_packs[CONFIG_CAN_PKG_COUNT];

__STATIC_INLINE can_pack_t *can_pack_get(u8 addr)
{
	can_pack_t *node, *node_end;
	can_pack_t *pack0, *pack1;

	pack1 = node = can_packs;
	pack0 = NULL;

	for (node_end = node + NELEM(can_packs); node < node_end; node++) {
		if (node->src == addr) {
			pack0 = node;
			continue;
		}

		if (node->times == 0xFFFF) {
			pack1 = node;
			continue;
		}

		if (node->times > pack1->times) {
			pack1 = node;
		}

		node->times++;
	}

	if (pack0 == NULL) {
		pack0 = pack1;
		pack0->src = addr;
		pack0->index = 0xFF;
	}

	pack0->times = 0;

	return pack0;
}

__STATIC_INLINE void can_pack_put(can_pack_t *pack)
{
	pack->times = 0xFFFF;
	pack->src = 0x00;
}


bool frame_rx_pack(can_frame_t *frame)
{
	can_pack_t *pack = can_pack_get(frame->src);
	u8 length = frame->length + 1;
	u8 total = frame->total;
	u8 index = frame->index;

	if (index != pack->index) {
		if (index != 1) {
			return false;
		}

		pack->type = frame->type;
		pack->dest = frame->dest;
		pack->length = 0;
		pack->index = 2;
	} else {
		pack->index = (index + 1) & 0x1F;
	}

	memcpy(pack->data + pack->length, frame->data, length);
	pack->length += length;

	if (total == index) {
		iap_process_pack(pack);
		can_pack_put(pack);
	}

	return true;
}

#endif
bool serial_on_frame_received(u8 *buff, u16 length)
{
	u16 crc0 = decode_u16(buff + length);
	u16 crc1 = crc16_get(buff, length);

	if (crc0 != crc1) {
		return false;
	}

#if CONFIG_SERIAL_TYPE == SERIAL_BOOTLOADER
	buff += 1;
	length -= 1; //skip 0xaa
	can_frame_t *frame = (can_frame_t *)buff;
	frame->length = length - 4 - 1; //reallen = length - 4, and frame->length = reallen - 1
	if (frame->dest != CONFIG_ADDR_ID) {
		return true;
	}
	if (length <= (8+4)) { //can frame in the serial data
		if (frame->dest == CONFIG_ADDR_ID){
			frame_rx_pack(frame);
		}
	}else {
		can_pack_t *pack = can_pack_get(0);
		if (pack == NULL) {
			return true;
		}
		pack->type = frame->type;
		pack->dest = frame->dest;
		pack->src = frame->src;
		pack->length = length - 4;
		memcpy(pack->data, buff + 4, pack->length);
		iap_process_pack(pack);
		can_pack_put(pack);
	}
#else
	can_frame_t *frame;
	if (buff[0] == 0x00) {
		frame = (can_frame_t *) (buff + 1);
		frame->length = length - 6;

		if (frame->dest == CONFIG_ADDR_ID && frame->src == 0x45) {
			frame_rx_pack(frame);
		} else {
			can_send_frame(frame->efid, frame->data);
		}
	} else if (buff[0] == 0x01) {
		u8 value = 0x00;
		serial_tx_command(0x01, &value, 1);
	} else if (buff[0] == 0x02) {
		serial_tx_command(0x02, iap_board_name, strlen(iap_board_name));
	} else {
		u8 value = 0x01;
		serial_tx_command(buff[0], &value, 1);
	}
#endif

	return true;
}
#if __CORTEX_M==0
#define CONFIG_FRAME_OFFSET 3
#else
#define CONFIG_FRAME_OFFSET 0
#endif
void serial_on_data_received(u8 *buff, u16 size)
{
	static bool escape = false;
	static u16 length = 0xFFFF;
	static u8 frame[256 + CONFIG_FRAME_OFFSET];
	u8 *buff_end;

	for (buff_end = buff + size; buff < buff_end; buff++) {
		u8 value = *buff;

		switch (value) {
		case CH_START:
			length = 0;
			escape = false;
			break;

		case CH_END:
			if (length > 2 && length != 0xFFFF) {
				serial_on_frame_received(frame + CONFIG_FRAME_OFFSET, length - 2);
			}
			length = 0xFFFF;
			break;

		case CH_ESC:
			escape = true;
			break;

		default:
			if (escape) {
				escape = false;

				switch (value) {
				case CH_ESC_START:
					value = CH_START;
					break;

				case CH_ESC_END:
					value = CH_END;
					break;

				case CH_ESC_ESC:
					value = CH_ESC;
					break;

				default:
					length = 0xFFFF;
				}
			}

			if (length < (sizeof(frame) - CONFIG_FRAME_OFFSET)) {
				frame[CONFIG_FRAME_OFFSET + length] = value;
				length++;
			} else {
				length = 0xFFFF;
			}
		}
	}
}

void serial_write(const u8 *buff, u16 size)
{
	while (size > 0) {
		u16 length = byte_queue_write(&uart_tx_queue, buff, size);

		if (length == size) {
			break;
		}

		uart_tx_poll();
		buff += length;
		size -= length;
	}
}

void serial_write_byte(u8 value)
{
	serial_write(&value, 1);
}

void serial_poll(void)
{
	uart_rx_poll();
	uart_tx_poll();
}


void serial_write_byte_esc(u8 value)
{
	switch (value) {
	case CH_START:
		serial_write_byte(CH_ESC);
		value = CH_ESC_START;
		break;

	case CH_END:
		serial_write_byte(CH_ESC);
		value = CH_ESC_END;
		break;

	case CH_ESC:
		serial_write_byte(CH_ESC);
		value = CH_ESC_ESC;
		break;
	}

	serial_write_byte(value);
}

void serial_write_esc(const u8 *buff, u16 length)
{
	const u8 *buff_end;

	for (buff_end = buff + length; buff < buff_end; buff++) {
		serial_write_byte_esc(*buff);
	}
}

void serial_tx_start(void)
{
	serial_write_byte(CH_START);
	serial_tx_crc16 = 0;
}

void serial_tx_continue(const void *buff, u16 length)
{
	serial_write_esc((const u8 *) buff, length);
	serial_tx_crc16 = crc16_update(serial_tx_crc16, (const u8 *) buff, length);
}

void serial_tx_end(void)
{
	serial_write_esc((u8 *) &serial_tx_crc16, sizeof(serial_tx_crc16));
	serial_write_byte(CH_END);
}

bool serial_tx_frame(u32 efid, const void *data)
{
	u8 length = ((efid >> 29) & 0x07) + 1;
#if CONFIG_SERIAL_TYPE == SERIAL_BOX
	u8 command = 0x00;
#endif

	serial_tx_start();

#if CONFIG_SERIAL_TYPE == SERIAL_BOX
	serial_tx_continue(&command, sizeof(command));
#endif

	serial_tx_continue(&efid, sizeof(efid));
	serial_tx_continue(data, length);
	serial_tx_end();

	return true;
}

bool serial_send_response(u8 dest, u8 src, const void *response, u16 length)
{
	u32 efid = 2 << 24 | 1 << 19 | 1 << 14 |src << 7 | dest;

	u8 command = 38;
	
	serial_tx_start();
	
	serial_tx_continue(&command, sizeof(command));
	
	serial_tx_continue(&efid, sizeof(efid));
	serial_tx_continue(response, length);
	serial_tx_end();

	return true;
}


#if CONFIG_SERIAL_TYPE == SERIAL_BOX
bool serial_tx_command(u8 command, const void *args, u16 length)
{
	serial_tx_start();
	serial_tx_continue(&command, sizeof(command));
	serial_tx_continue(args, length);
	serial_tx_end();

	return true;
}
#endif
#endif

